#define _LEGACY_HEADERS

#include <htc.h>
#include <pic.h>
#include "Configuration.h"


#define RISING_EDGE	1
#define FALLING_EDGE	0
#define _128SPEED	1
#define	_28SPEED	0
#define PRESFOSC	19.660
#define	TMR0POSTSCALER	4

// DCC TIMMING CONSTANTS
#define	_1UPPER_LIMIT	(unsigned char)(0.5+(70*PRESFOSC)/(4*TMR0POSTSCALER))  //86 dec  // 70 us
#define	_1LOWER_LIMIT	(unsigned char)(0.5+(40*PRESFOSC)/(4*TMR0POSTSCALER))  //49 dec  // 40 us
#define	_0LOWER_LIMIT	(unsigned char)(0.5+(93*PRESFOSC)/(4*TMR0POSTSCALER))  //114 dec // 93 us
#define	_0UPPER_LIMIT	(unsigned char)(0.5+(200*PRESFOSC)/(4*TMR0POSTSCALER))  //246 // 200 us

// DCC STATE MACHINE CONSTANTS
#define PREAMBLE_STATE			0
#define WAIT_INIT_BIT			1  // Init packet bit
#define	FIRST_BYTE_STATE		2
#define	WAIT_START1_BIT			3
#define	SECOND_BYTE_STATE		4
#define	WAIT_START2_BIT			5
#define	THIRD_BYTE_STATE 		6
#define	WAIT_START3_BIT			7
#define	FORTH_BYTE_STATE 		8
#define	WAIT_START4_BIT			9
#define	FIFTH_BYTE_STATE 		10
#define	WAIT_START5_BIT			11


/****** DCC PACKET COMMANDS (SEE 9.2.1 NMRA Standard) ***************/
#define	DECODER_CTRL_CMD		0b00000000	// 0000XXXX
#define	CONSIST_CTRL_CMD		0b00010000	// 0001XXXX
#define ADV_INSTRUCT_CMD		0b00100000	// 001XXXXX
#define SPEED_128_CMD			0b00111111	// 00111111  Packet for 128 Steps Configuration
#define	SPEED_DIR_CMD			0b01000000	// 01XXXXXX   (010=Reverese   011=Forward)
#define	SPEED_DIR_CMD2			0b01100000	// 01XXXXXX   (010=Reverese   011=Forward)
#define FUNC_GRP1_CMD			0b10000000	// 100XXXXX
#define FUNC_GRP2_CMD			0b10100000	// 101XXXXX
#define	FUNC_GRP2_A_CMD			0b10110000	// 1011XXXX	  (F5-F8)
#define	FUNC_GRP2_B_CMD			0b10100000	// 1010XXXX		(F9-F12)
#define	FEATURE_EXP_CMD			0b11000000	// 110XXXXX
#define	CV_ACCESS_CMD			0b11100000	// 111XXXXX	 (Represented in the next to commands
#define	CV_WRITE_CMD			0b11101100	// 11101100  (CV Access - WriteByte)
#define	CV_READ_CMD			0b11100100	// 11100100  (CV Access - VerifyByte)
#define	CV_BIT_WRITE_CMD		0b11101000	// 11100100  (CV Access - Bit Manipulation)
#define	CV_WRITE_DMODE_CMD              0b01111100	// 011111XX  (CV Access Direct Mode - WriteByte)
#define	CV_READ_DMODE_CMD		0b01110100	// 011101XX  (CV Access Direct Mode - ReadByte)
#define	CV_WRITE_PHYS_REG_CMD           0b01111000	// 01111RRR  (CV Physical Register instruction- WriteByte)
#define	CV_WRITE_ADDR_ONLY_MODE_CMD	0b01111000	// 01111XXX  (CV Access Address Only Mode- WriteByte)
#define	CV_READ_ADDR_ONLY_MODE_CMD	0b01110000	// 011100XX  (CV Access Address Only Mode - WriteByte)
#define RESET_CMD			0b00000000	// 00000000  Reset command (Must stop inmediatly
#define SERVICE_LOCK_CMD		0b11111001  // Service lock command 0xFA (See RP 9.2.3 Apendix B)

/*****  M A C R O   D E F I N I T I O N S ********************/
#define HIGH(x) 	((unsigned)(x) >> 8)
#define LOW(x)  	((x) & 0xFF)
#define BTF(x,b)	((x) ^= (1 << (b)))	// Bit Toggle
#define BCF(x,b)	((x) &= ~(1 << (b)))	// Bit Clear
#define BSF(x,b)	((x) |= (1 << (b)))	// Bit Set
#define BTSTF(x,b)	(!!((x) & (1 << (b))))  // Bit test

/*********** D A T A   S T R U C T U R E S*********************/
typedef union {
    unsigned char _byte;

    struct {
        unsigned DCC_bit_state : 1;
        unsigned CheckHalfBit : 1;
        unsigned FirstHalfBit : 1;
        unsigned BitReceived : 1; // stores the second half bit and Stores the bit received from DCC according to timming
        unsigned CheckFullBit : 1; // Indicates a complete bit has been received OK (Timing is ok)
        unsigned Ack : 1; // Indicates it has to accknowledge the command
        unsigned MustStop : 1; // Emergency stop
        unsigned DCMode : 1; // Indicates the loco is operating in DC mode
        unsigned DCCServiceMode : 1; // Idicates a ServiceMode reset has been received
        unsigned RecalcPWM : 1; // Must calculate the PWM Output timming in Main
        unsigned MayProcServMode : 1; // Flag to allow AddressOnly Mode andPhysical Register Addressing Service Mode instructions
        unsigned CheckLights : 1; // Must check Lights in Main. Also used to check the Accel Decel timming
        unsigned MustProcessCmd : 1; // Indicates that a valid DCC packet has been anailzed and allow the main routine to recaluculate PWM out and other time consuming tasks

    } flag;
} TDCCFlags;

typedef union {
    char _byte;

    struct {
        unsigned DirBit : 1;
        unsigned RealDirBit : 1; // To allow deceleration and changing dir
        unsigned SpeedMode : 1;
    } bits;
} TLocoFlags;

/****** Configuration of CV29 **************/
typedef union{
	char	_byte;
	struct {
		unsigned	LocoDirection		:	1;
		unsigned	FL_location			: 1;
		unsigned	PowerConvertion	: 1;
		unsigned	Bi_comunication	:	1;
		unsigned	SpeedTable			: 1;
		unsigned	ExtAddressing		: 1;
		unsigned	Reserved				: 1;
		unsigned	TypeOfDecoder		: 1; // 0=Multifunctional  1=Accesory Decoder (See CV#541 for description for bits 0-6
	} flags;
} TCV29;

typedef struct {
    char Address; // CV1
    char CV2; // VStart
    char CV3; // Acceleration Rate
    char CV4; // Deceleration Rate
    char CV5; // VHigh
    char CV6; // VMid (Apply only on 128 Speed Step)
    char CV7; // Manufacturer Version
    char CV8; // Manufactured ID
    char CV11; // Packet Time out Value to stop when not receiving a DCC packet address to the decoder
    char CV19; // Consist Address
    TCV29 CV29;	// Configuration Variable
    char Speed;
    TLocoFlags Flags;
    //TFuncGroup	Func1;
    //TFuncGroup	Func2;
} TLocoData;


TDCCFlags DCCFlags;
TLocoData LocoData;

/** DCC Variables **/

char buffer[5]; // Contains the bytes received from DCC (Max number of bytes)
char DCC_Byte; // Where the program stores the bits received
char DCC_State;
char BytesReceived;
char BitsReceived; // To check how many bits has been received in a normal byte
char PreambleBits; // Counter to detect the preamble bits
char HalfBitTime; // To save TMR1L value that represents the time taken by the half bit to change state

void interrupt int_vct(void) {
    char BitEdgeStatus;

    if (TMR1IF == 1) {
        TMR1IF = 0;
        DCCFlags.flag.DCC_bit_state = 0;
        RA2 ^= 1;

        PEIE = 1;
        TMR1IE = 1;
        CCP1IE = 1;
        CMCON = 0x07;

        if (DCC_Input == 1) {
            CCP1CON = 0b00000100;
        } else {
            CCP1CON = 0b00000101;
        }

        T1CON = 0b00100101; // Preescaler x 4
        TMR1H = 0;
        TMR1L = 0;

    }

    if (CCP1IF == 1) {
        CCP1IF = 0;
        TMR1H = 0;
        TMR1L = 0;

        if (DCC_Input == 1) {
            BitEdgeStatus = 0;
        } else {
            BitEdgeStatus = 1;
        }

        CCP1CON = 0;
        CCP1CON = 0b00000100 + BitEdgeStatus;

        HalfBitTime = CCPR1L;
        DCCFlags.flag.CheckHalfBit = 1;

    }
}

/******************************************************************************
 * Function:        void InitDCCState(void);
 * PreCondition:    None
 * Input:
 * Output:
 * Side Effects:    None
 * Overview:        Initialize the DCC Variables
 * Note:            None
 *****************************************************************************/
void InitDCCState(void) {
    // ***** Initiates DCC States *****
    DCCFlags._byte = 0;
    DCCFlags.flag.DCMode = 0; // Assumes is operating on DCC. Changes when received the first DCC bit
    LocoData.Flags.bits.SpeedMode = _28SPEED;
    PreambleBits = 0;
    DCC_Byte = 0;
    DCC_State = 0;
    BytesReceived = 0;
}

/******************************************************************************
 * Function:        unsigned char DecodeSpeed(char CodedSpeed)
 * PreCondition:    None
 * Input:           None
 * Output:          None
 * Side Effects:    None
 * Overview:        Decode the received Speed from DCC, depending
 *									wether it is 28 or 128.
 *
 * Note:
 *****************************************************************************/
unsigned char DecodeSpeed(char CodedSpeed) {
	unsigned char result;

	LocoData.Flags.bits.DirBit = 0; //Assumes forward direction
	if (LocoData.Flags.bits.SpeedMode == _28SPEED) {
		if (BTSTF(CodedSpeed,5)==1)
			LocoData.Flags.bits.DirBit =1; // Change to reverse direction
		result = (CodedSpeed & 0b00001111) << 1;
		if (BTSTF(CodedSpeed,4)) {
			result++;  // Sets the LSb of the Speed
		}
	} else {
		if (BTSTF(buffer[2],7)==1) {
			LocoData.Flags.bits.DirBit=1; // Change to reverse direction
		}
		result = CodedSpeed & 0x7F;// Masks the 7 bit speed
	}

	if (LocoData.CV29.flags.LocoDirection == 1) {// Reversing mode
	  LocoData.Flags.bits.DirBit ^= 1; // Toggle Direction Bit
	}

	DCCFlags.flag.RecalcPWM = 1; // Flag to calculate the PWM time ON in Main routine
	return (result);

}

/*******
 * Funtion : void AnalizeDCC(void)
 * Overview: Decode a received DCC Packet
 ********/

void AnalizeDCC(void) {
    char TempCommand; // To help decoding DCC commands
    char PhysicalRegister; // To decode Physical Register Service Mode Command
    volatile char CheckSum;
    volatile char i;

    CheckSum = 0;

    for (i = 0; i < BitsReceived - 1; i++) {
        CheckSum ^= buffer[i];
    }

    if (CheckSum == buffer[i]) {
        if (PreambleBits <= 20) {
            if (buffer[0] == LocoData.Address) {
                TempCommand = buffer[i] & 0b11100000;

                switch (TempCommand) {
                    case SPEED_DIR_CMD: case SPEED_DIR_CMD2:{
                        LocoData.Flags.bits.SpeedMode = _28SPEED;
                        LocoData.Speed = DecodeSpeed(buffer[1] & 0b00111111); // Ensures that no more than 32 value comes here
                        break;
                    }
                }
            }
        }
    }

}


void main() {

    initPorts();
    CMCON = 0x07;

    PEIE = 1;
    TMR1IE = 1;
    CCP1IE = 1;

    if (DCC_Input == 1) {
        CCP1CON = 0b00000100; //Capture Falling edge
    } else {
        CCP1CON = 0b00000101; //Capture Rising edge
    }

    T1CON = 0b00100101; // Preescaler x 2

    InitDCCState();
    GIE = 1;

    while (1) {

        if (DCCFlags.flag.CheckHalfBit == 1) {
            DCCFlags.flag.CheckHalfBit = 0;

            if (DCCFlags.flag.DCC_bit_state == 0) {
                if ((HalfBitTime > _1LOWER_LIMIT) && (HalfBitTime < _1UPPER_LIMIT)) {
                    DCCFlags.flag.FirstHalfBit = 1;
                } else {
                    DCCFlags.flag.FirstHalfBit = 0;
                }
            } else {
                if ((HalfBitTime > _1LOWER_LIMIT) && (HalfBitTime < _1UPPER_LIMIT)) {
                    DCCFlags.flag.BitReceived = 1;
                } else {
                    DCCFlags.flag.BitReceived = 0;
                }

                if (DCCFlags.flag.BitReceived == DCCFlags.flag.FirstHalfBit) {
                    DCCFlags.flag.CheckFullBit = 1;

                } else {
                    if (DCCFlags.flag.FirstHalfBit == 0) {
                        DCCFlags.flag.DCC_bit_state = 1;
                    } else {
                        DCCFlags.flag.DCC_bit_state = 0;
                    }

                    DCCFlags.flag.FirstHalfBit = DCCFlags.flag.BitReceived;
                    DCC_State = 0;
                    PreambleBits = 0;
                }
            }

            DCCFlags.flag.DCC_bit_state ^= 1;

            if (DCCFlags.flag.CheckFullBit == 1) {
                DCCFlags.flag.CheckFullBit = 0;

                switch (DCC_State) {
                    case PREAMBLE_STATE:
                    {
                        if (DCCFlags.flag.BitReceived == 1) {
                            PreambleBits++;
                        } else {
                            PreambleBits = 0;
                        }

                        if (PreambleBits >= 10) {
                            DCC_State++;
                        }
                        break;
                    }

                    case WAIT_INIT_BIT:
                    {
                        if (DCCFlags.flag.BitReceived == 1) {
                            PreambleBits++; // Continue incrementing Preamble bits for Long preamble packets
                        } else {
                            DCC_State++; // A Start (0) bit has been received
                            BytesReceived = 0;
                            buffer[0] = 0;
                            buffer[1] = 0;
                            buffer[2] = 0;
                            buffer[3] = 0;
                            buffer[4] = 0;
                        }
                        break;
                    }

                    case FIRST_BYTE_STATE:
                    {
                        buffer[0] |= DCCFlags.flag.BitReceived;
                        BitsReceived++;
                        if (BitsReceived < 8) {
                            buffer[0] = buffer[0] << 1;
                        } else {
                            DCC_State++; // Checks for the 0 bit between bytes
                            BitsReceived = 0;
                            BytesReceived++;

                        }
                        break;
                    }

                    case WAIT_START1_BIT:
                    {
                        if (DCCFlags.flag.BitReceived == 0) {
                            DCC_State++;
                        } else {
                            DCC_State = 0; // An error on the protocol ocurred so restarts the packet
                            PreambleBits = 0;
                        }
                        break;
                    }

                    case SECOND_BYTE_STATE:
                    {
                        buffer[1] |= DCCFlags.flag.BitReceived;
                        BitsReceived++;
                        if (BitsReceived < 8) {
                            buffer[1] = buffer[1] << 1;
                        } else {
                            DCC_State++; // Checks for the 0 bit between bytes
                            BitsReceived = 0;
                            BytesReceived++;
                        }
                        break;
                    }

                    case WAIT_START2_BIT:
                    {
                        if (DCCFlags.flag.BitReceived == 0) {
                            DCC_State++;
                        } else {
                            DCC_State = 0; // An error on the protocol ocurred so restarts the packet
                            PreambleBits = 0;
                        }
                        break;
                    }

                    case THIRD_BYTE_STATE:
                    {
                        buffer[2] |= DCCFlags.flag.BitReceived;
                        BitsReceived++;
                        if (BitsReceived < 8) {
                            buffer[2] = buffer[2] << 1;
                        } else {
                            DCC_State++; // Checks for the 0 bit between bytes
                            BitsReceived = 0;
                            BytesReceived++;
                        }
                        break;
                    }

                    case WAIT_START3_BIT:
                    {
                        if (DCCFlags.flag.BitReceived == 0) {
                            DCC_State++;
                        } else {
                            AnalizeDCC();
                            DCC_State = 0; // An error on the protocol ocurred so restarts the packet
                            PreambleBits = 0;
                        }
                        break;
                    }

                    case FORTH_BYTE_STATE:
                    {
                        buffer[3] |= DCCFlags.flag.BitReceived;
                        BitsReceived++;
                        if (BitsReceived < 8) {
                            buffer[3] = buffer[3] << 1;
                        } else {
                            DCC_State++; // Checks for the 0 bit between bytes
                            BitsReceived = 0;
                            BytesReceived++;
                        }
                        break;
                    }

                    case WAIT_START4_BIT:
                    {
                        if (DCCFlags.flag.BitReceived == 0) {
                            DCC_State++;
                        } else {
                            AnalizeDCC();
                            DCC_State = 0; // An error on the protocol ocurred so restarts the packet
                            PreambleBits = 0;
                        }
                        break;
                    }

                    case FIFTH_BYTE_STATE:
                    {
                        buffer[4] |= DCCFlags.flag.BitReceived;
                        BitsReceived++;
                        if (BitsReceived < 8) {
                            buffer[4] = buffer[4] << 1;
                        } else {
                            DCC_State++; // Checks for the 0 bit between bytes
                            BitsReceived = 0;
                            BytesReceived++;
                        }
                        break;
                    }

                    case WAIT_START5_BIT:
                    {
                        if (DCCFlags.flag.BitReceived == 1) { // End Packet Bit (
                            AnalizeDCC();
                            DCC_State = 0;
                        } else {
                            DCC_State = 0; // Resets the Packet State because It didnt received and 1 end bit
                            PreambleBits = 0;
                        }
                        break;
                    }

                    default: DCC_State = 0;
                }
            }

        }
    }

}
